#ifndef __placemedicine_H
#define __placemedicine_H
#include <stdint.h>
#include <main.h>
#include <math.h>
#include "steer.h"
#include "dji_ctr.h"
#include "voice.h"
#include "point.h"
typedef enum
{
    PLACEMEDICINE_NONE = 0,
    SCAN,
    OPEN_BOX,               // 打开药盒
    SEND_GRIP,              // 送出机械爪
    LOOSEN_GRIP,            // 松开机械爪
    CLOSE_GRIP,             // 合上机械爪
    RECIEVE_GRIP,           // 收回机械爪
    CLOSE_BOX,              // 关闭药盒
    PM_WAIT_STATUS          // 等待状态(延时)
} PLACEMEDICINE_STATUS;

extern uint32_t PM_start_flag;
extern PLACEMEDICINE_STATUS place_medicine1, place_medicine3, next_PM_status;
extern uint32_t PM_num;
extern uint32_t open_box_angle;
extern uint32_t close_box_angle;
extern uint32_t loosen_grip_angle;
extern uint32_t close_grip_angle;
extern int left_conveyor_belt_pos;
extern int right_conveyor_belt_pos;
extern uint32_t PM_wait_ms;
extern uint32_t PM_cnt_ms;

/* 左机械爪 闭合打开角度 */
#define LEFT_GRIP_CLOSE_ANGLE   35//32
#define LEFT_GRIP_LOOSE_ANGLE   45//40

/* 右机械爪 闭合打开角度 */
#define RIGHT_GRIP_CLOSE_ANGLE  48//41
#define RIGHT_GRIP_LOOSE_ANGLE  65//52

/* 左药盒 闭合打开角度 */
#define LEFT_BOX_CLOSE_ANGLE    107
#define LEFT_BOX_OPEN_ANGLE     80

/* 右药盒 闭合打开角度 */
#define RIGHT_BOX_CLOSE_ANGLE   100
#define RIGHT_BOX_OPEN_ANGLE    73

void loose_grip();
void close_grip();
void open_box();
void close_box();
void send_grip();
void recieve_grip();
uint8_t scan(int num);
uint8_t rescan();

uint8_t grip_is_arrive(int targetpos);
void PM_state_machine();

void PM_init();
void PM_exe();

void test_PM(int num);
#endif